package es.uji.viselab.robot;

import java.awt.Button;
import java.awt.FlowLayout;
import java.awt.GridLayout;
import java.awt.Label;
import java.awt.Panel;
import java.awt.Scrollbar;
import java.awt.TextField;
import java.awt.event.ActionEvent;
import java.awt.event.ActionListener;
import java.awt.event.AdjustmentEvent;
import java.awt.event.AdjustmentListener;
import java.awt.event.MouseEvent;
import java.awt.event.MouseListener;

import javax.swing.BoxLayout;
import javax.swing.JPanel;
import javax.swing.JScrollBar;

import es.uji.viselab.ViSeLabComponent;
import es.uji.viselab.ViSeLabException;
import es.uji.viselab.math.Joints;
import es.uji.viselab.math.Pose;
import es.uji.viselab.util.LogLevel;
import es.uji.viselab.util.LogRecorder;

public class ControlInterface implements ActionListener, AdjustmentListener, MouseListener,
		RobotListener, ViSeLabComponent {

	Robot robot;
	int numJoints;

	JPanel rootPanel;
	Panel buttonsPanel = new Panel();
	Panel controlPanelLine1 = new Panel();
	Panel controlPanelLine2 = new Panel();
	Panel scrollBarsPanel = new Panel();

	Panel scrollBarsPanelJoints = new Panel();
	Panel[] jointPanel = new Panel[6];
	Label[] jointLabel = new Label[6];
	JScrollBar[] jointScrollbar = new JScrollBar[6];
	TextField[] jointScrollbarTextValue = new TextField[6];
	TextField[] jointTextValue = new TextField[6];

	Panel scrollBarsPanelXYZ = new Panel();
	Panel[] XYZPanel = new Panel[6];
	Label[] XYZLabel = new Label[6];
	JScrollBar[] XYZScrollbar = new JScrollBar[6];
	TextField[] XYZScrollbarTextValue = new TextField[6];
	TextField[] XYZTextValue = new TextField[6];

	Label labelIPRobotServer = new Label("IP of robot server: ");
	TextField textIPRobotServer = new TextField(12);
	Label labelPortRobotServer = new Label("Port of robot server:");
	TextField textPortRobotServer = new TextField(5);

	Label labelJoints = new Label("            Joints                  ");
	Label labelXYZWPR = new Label("Position (XYZ), Tool orientation (XYZ)");

	Button connectRobotButton = new Button("Connect Robot");
	Button moveBasePositionButton = new Button("Move starting position");
	Button disconnectRobotButton = new Button("Disconnect Robot");
	Button isConnectedButton = new Button("Is connected?");
	Button pingRobotServerButton = new Button("Ping Robot Server");

	public ControlInterface(Robot robot) {
		this.robot = robot;
		this.numJoints = robot.getCurrentRobotModel().getNumberJoints();
		constructPanel();
	}

	protected void constructPanel() {

		rootPanel = new JPanel();
		rootPanel.setLayout(new BoxLayout(rootPanel, BoxLayout.PAGE_AXIS));

		controlPanelLine1.setLayout(new FlowLayout());

		controlPanelLine1.add(connectRobotButton);
		controlPanelLine1.add(pingRobotServerButton);
		controlPanelLine1.add(isConnectedButton);
		controlPanelLine1.add(disconnectRobotButton);
		if (robot.getCurrentRobotModel().isReal()) {
			controlPanelLine1.add(labelIPRobotServer);
			controlPanelLine1.add(textIPRobotServer);
			controlPanelLine1.add(labelPortRobotServer);
			controlPanelLine1.add(textPortRobotServer);
		}

		controlPanelLine2.setLayout(new FlowLayout());
		controlPanelLine2.add(moveBasePositionButton);

		scrollBarsPanel.setLayout(new GridLayout(1, 2));
		scrollBarsPanelJoints.setLayout(new GridLayout(7, 1));
		scrollBarsPanelXYZ.setLayout(new GridLayout(7, 1));

		buttonsPanel.setLayout(new GridLayout(2, 1));
		buttonsPanel.add(controlPanelLine1);
		buttonsPanel.add(controlPanelLine2);
		buttonsPanel.add(scrollBarsPanel);
		rootPanel.add(buttonsPanel);

		scrollBarsPanel.add(scrollBarsPanelJoints);
		scrollBarsPanel.add(scrollBarsPanelXYZ);

		scrollBarsPanelJoints.add(labelJoints);

		for (int i = 0; i < numJoints; i++) {
			jointPanel[i] = new Panel();
			jointPanel[i].setLayout(new FlowLayout());
			jointLabel[i] = new Label("j" + (i + 1));
			jointScrollbar[i] = new JScrollBar(Scrollbar.HORIZONTAL, 0, 1, -360, 360);
			jointScrollbar[i].setUnitIncrement(1);
			jointScrollbarTextValue[i] = new TextField(4);
			jointTextValue[i] = new TextField(4);
			jointScrollbarTextValue[i].setText("0");

			jointPanel[i].add(jointLabel[i]);
			jointPanel[i].add(jointScrollbar[i]);
			jointPanel[i].add(jointScrollbarTextValue[i]);
			jointPanel[i].add(jointTextValue[i]);

			scrollBarsPanelJoints.add(jointPanel[i]);
		}

		scrollBarsPanelXYZ.add(labelXYZWPR);

		for (int i = 0; i < 6; i++) {
			XYZPanel[i] = new Panel();
			XYZPanel[i].setLayout(new FlowLayout());
			XYZLabel[i] = new Label();
			if (i == 0 || i == 3)
				XYZLabel[i].setText("x");
			else if (i == 1 || i == 4)
				XYZLabel[i].setText("y");
			else if (i == 2 || i == 5)
				XYZLabel[i].setText("z");
			XYZScrollbar[i] = new JScrollBar(Scrollbar.HORIZONTAL, 10, 20, -1000, 2000);
			XYZScrollbar[i].setUnitIncrement(10);
			XYZScrollbarTextValue[i] = new TextField(4);
			XYZTextValue[i] = new TextField(4);
			XYZScrollbarTextValue[i].setText("0");

			XYZPanel[i].add(XYZLabel[i]);
			XYZPanel[i].add(XYZScrollbar[i]);
			XYZPanel[i].add(XYZScrollbarTextValue[i]);
			XYZPanel[i].add(XYZTextValue[i]);
			scrollBarsPanelXYZ.add(XYZPanel[i]);
		}

		rootPanel.add(scrollBarsPanel);

		connectRobotButton.addActionListener(this);
		disconnectRobotButton.addActionListener(this);
		pingRobotServerButton.addActionListener(this);
		isConnectedButton.addActionListener(this);
		moveBasePositionButton.addActionListener(this);
		textIPRobotServer.setText("192.168.125.1");
		textPortRobotServer.setText("1025");
	}

	public JPanel getPanel() {

		return rootPanel;
	}

	public void initializeValues() {

		getAndPrintXYZPosition();
		for (int i = 0; i < 6; i++) {

			// Initialitzation of Scrollbar with the current robot position
			XYZScrollbar[i].setValue(Integer.parseInt(XYZTextValue[i].getText()));
			// Only after initializing values, it is convenient to add the
			// listener to the Scrollbar
			XYZScrollbarTextValue[i].setText(XYZTextValue[i].getText());
			XYZScrollbar[i].addAdjustmentListener(this);

		}
		getAndPrintJointsValues();
		for (int i = 0; i < numJoints; i++) {
			// Initialitzation of Scrollbar with the current robot position
			jointScrollbar[i].setValue((int) Double.parseDouble(jointTextValue[i].getText()));
			jointScrollbarTextValue[i].setText(jointTextValue[i].getText());
			// Only after initializing values, it is convenient to add the
			// listener to the Scrollbar
			jointScrollbar[i].addAdjustmentListener(this);
		}
	}

	@Override
	public void actionPerformed(ActionEvent e) {

		if (e.getSource().equals(moveBasePositionButton)) {
			double[] v = robot.getCurrentRobotModel().getDefaultJointsPosition();
			Joints j = new Joints(v);
			try {
				robot.move(j);
			} catch (RobotException e1) {
				e1.printStackTrace();
			}
			initializeValues(); // print the new coordinates values
		}
		if (e.getSource().equals(connectRobotButton)) {
			connectRobot();
		}

		if (e.getSource().equals(disconnectRobotButton)) {
			try {
				LogRecorder.log(LogLevel.INFO, "Disconnecting...");
				robot.disconnect();
				LogRecorder.log(LogLevel.INFO, "Disconnected...");
			} catch (ViSeLabException ke) {
				LogRecorder.log(LogLevel.ERROR, ke.getMessage());
			}
		}

		if (e.getSource().equals(isConnectedButton)) {

			if (robot.isConnected()) {
				LogRecorder.log(LogLevel.INFO, "Robot is connected");
			} else {
				LogRecorder.log(LogLevel.INFO, "Robot is not connected");
			}
		}

		if (e.getSource().equals(pingRobotServerButton)) {
			try {
				LogRecorder.log(LogLevel.INFO, "ping");
				LogRecorder.log(LogLevel.INFO, robot.ping());
			} catch (ViSeLabException ke) {
				LogRecorder.log(LogLevel.INFO, "ping Error: " + ke.getMessage());
			}
		}
	}

	private void connectRobot() {
		try {
			String IP = textIPRobotServer.getText();
			int Port = Integer.parseInt(textPortRobotServer.getText());
			LogRecorder.log(LogLevel.INFO, "Connecting to robot... (" + IP + ":"+ Port + ")");

			if (!robot.isVirtual()) {
				robot.connect(IP, Port);
			} else {
				robot.connect(null, 0);
				LogRecorder.log(LogLevel.DETAIL, "Using virtual interface");
			}

			LogRecorder.log(LogLevel.INFO, "Robot connected!");

			// Get position values from the robot and print them
			initializeValues();

		} catch (ViSeLabException ke) {
			LogRecorder.log(LogLevel.ERROR, ke.getMessage());
		}
	}

	public void adjustmentValueChanged(AdjustmentEvent e) {

		if (e.getSource().equals(jointScrollbar[0]) || e.getSource().equals(jointScrollbar[1])
				|| e.getSource().equals(jointScrollbar[2])
				|| e.getSource().equals(jointScrollbar[3])
				|| e.getSource().equals(jointScrollbar[4])
				|| e.getSource().equals(jointScrollbar[5])) {
			for (int i = 0; i < 6; i++) {
				jointScrollbarTextValue[i].setText(String.valueOf(jointScrollbar[i].getValue()));
			}

			Joints j = new Joints(jointScrollbar[0].getValue() * 2 * Math.PI / 360,
					jointScrollbar[1].getValue() * 2 * Math.PI / 360, jointScrollbar[2].getValue()
							* 2 * Math.PI / 360, jointScrollbar[3].getValue() * 2 * Math.PI / 360,
					jointScrollbar[4].getValue() * 2 * Math.PI / 360, jointScrollbar[5].getValue()
							* 2 * Math.PI / 360);
			try {
				robot.move(j);
			} catch (ViSeLabException ke) {
				LogRecorder.log(LogLevel.ERROR, ke.getMessage());
			}
		}

		if (e.getSource().equals(XYZScrollbar[0]) || e.getSource().equals(XYZScrollbar[1])
				|| e.getSource().equals(XYZScrollbar[2]) || e.getSource().equals(XYZScrollbar[3])
				|| e.getSource().equals(XYZScrollbar[4]) || e.getSource().equals(XYZScrollbar[5])) {

			for (int i = 0; i < 6; i++) {
				XYZScrollbarTextValue[i].setText(String.valueOf(XYZScrollbar[i].getValue()));
			}
			double[] v = { XYZScrollbar[0].getValue(), XYZScrollbar[1].getValue(),
					XYZScrollbar[2].getValue(), XYZScrollbar[3].getValue(),
					XYZScrollbar[4].getValue(), XYZScrollbar[5].getValue() };
			for (int i = 0; i < 6; i++)
				v[i] = v[i] / 1000;
			try {
				robot.setPosition(v);
			} catch (RobotException re) {
				LogRecorder.log(LogLevel.ERROR, re.getMessage());
			}
		}

	}

	@Override
	public void mouseClicked(MouseEvent arg0) {
		// TODO Auto-generated method stub

	}

	@Override
	public void mouseEntered(MouseEvent arg0) {
		// TODO Auto-generated method stub

	}

	@Override
	public void mouseExited(MouseEvent arg0) {
		// TODO Auto-generated method stub

	}

	@Override
	public void mousePressed(MouseEvent arg0) {
		// TODO Auto-generated method stub

	}

	@Override
	public void mouseReleased(MouseEvent arg0) {
		// TODO Auto-generated method stub

	}

	
	private void getAndPrintJointsValues() {
		Joints j;
		try {
			j = robot.getJoints();
			if (j != null)
				printJointsValues(j);
		} catch (ViSeLabException e) {
			LogRecorder.log(LogLevel.ERROR, "printJointsValues ERROR: " + e.getMessage());
		}

	}

	private void printJointsValues(Joints j) {
		int precision = 2;

		for (int i = 0; i < 6; i++)
			if (j != null)
				jointTextValue[i].setText(j.getJointToString(i + 1, precision));
			else
				jointTextValue[i].setText("0");
	}

	private void getAndPrintXYZPosition() {
		Pose pose;
		pose = robot.getPose();
		printXYZPosition(pose);

	}

	private void printXYZPosition(Pose pose) {

		XYZTextValue[0].setText(String.valueOf((int) (pose.getX() * 1000)));
		XYZTextValue[1].setText(String.valueOf((int) (pose.getY() * 1000)));
		XYZTextValue[2].setText(String.valueOf((int) (pose.getZ() * 1000)));
		XYZTextValue[3].setText(String.valueOf((int) (pose.getOrientationX())));
		XYZTextValue[4].setText(String.valueOf((int) (pose.getOrientationY())));
		XYZTextValue[5].setText(String.valueOf((int) (pose.getOrientationZ())));
	}

	@Override
	public void robotPositionChanged(Robot robot) {
		// TODO Auto-generated method stub

	}

	
	@Override
	public void robotConnected(Robot robot) {
	}

	

	@Override
	public String getPanelTitle() {
		return "Control";
	}

	@Override
	public int panelWidth() {
		return 600;
	}

	@Override
	public int panelHeight() {
		return 400;
	}

	public void autoStart() {
		connectRobot();
	}

	

}